/*

	需求：实现订阅到目标消息后将消息通过串口传输出去
		1.订阅到机械臂抓取状态发生改变后将消息传递到开发板端
	实现流程：
		1.包含头文件
		2.初始化ros节点
		3.实例化ros句柄
		4.实例化订阅者对象
		5.处理订阅的消息（回调函数）
		6.开启串口实现
		7.设置循环调用回调函数

*/

// 1.包含头文件
#include <string>
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>

serial::Serial _serial; // serial object

// 实现回调函数
void arm_status_callback(const std_msgs::String::ConstPtr &msg)
{
	if (!msg->data.empty())
	{
		_serial.write(msg->data);
		ROS_INFO_STREAM("The SwiftproState is " << msg->data.c_str());
	}
}

int main(int argc, char **argv)
{
	// 2.初始化hm_write_node节点
	ros::init(argc, argv, "hm_write_node");

	// 3.实例化ros句柄
	ros::NodeHandle nh;

	// 4.实例化订阅者对象
	ros::Subscriber sub = nh.subscribe<std_msgs::String>("/grasp_status", 1, arm_status_callback);
	
	// 5.处理订阅的消息（回调函数）
	// 开启串口实现
	try
	{
		_serial.setPort("/dev/SPARK-HM-PI");
		_serial.setBaudrate(115200);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		_serial.setTimeout(to);
		_serial.open();
		ROS_INFO_STREAM("Port has been open successfully");
	}
	catch (serial::IOException &e)
	{
		ROS_INFO_STREAM("Unable to open port");
		return -1;
	}

	if (_serial.isOpen())
	{
		ROS_INFO_STREAM("Port is open ");
	}

	// 7.设置循环调用回调函数
	ros::spin();
	return 0;
}
